/**************************************************************************************************
* Title: SumVector3_16f.inl
* Author: Gael Huber
* Description: Implements 16-byte aligned 3D vector functions.
**************************************************************************************************/

/**************************************************************************************************
* Default constructor
**************************************************************************************************/
inline Vector3_16f::Vector3_16f(void)
	: v(_mm_setzero_ps())
{ }

/**************************************************************************************************
* Constructor specifying vector
**************************************************************************************************/
inline Vector3_16f::Vector3_16f(__m128 f)
	: v(f)
{ }

/**************************************************************************************************
* Constructor from array
**************************************************************************************************/
inline Vector3_16f::Vector3_16f(const float* f)
	: v(_mm_set_ps(0.0f, f[2], f[1], f[0]))
{ }

/**************************************************************************************************
* Constructor from specified values
**************************************************************************************************/
inline Vector3_16f::Vector3_16f(float x, float y, float z)
	: v(_mm_set_ps(0.0f, z, y, x))
{ }

/**************************************************************************************************
* Copy constructor
**************************************************************************************************/
inline Vector3_16f::Vector3_16f(const Vector3_16f& rhs)
	: v(rhs.v)
{ }

/**************************************************************************************************
* Destructor
**************************************************************************************************/
inline Vector3_16f::~Vector3_16f(void)
{ }

/**************************************************************************************************
* Returns the data as a float array
**************************************************************************************************/
inline Vector3_16f::operator float*(void) {
	return (float*) this;
}

/**************************************************************************************************
* Returns the data as a const float array
**************************************************************************************************/
inline Vector3_16f::operator const float*(void) const {
	return (const float*) this;
}

/**************************************************************************************************
* Assignment operator
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator=(const Vector3_16f& rhs) {
	if(this == &rhs)
		return *this;

	v = rhs.v;
	return *this;
}
/**************************************************************************************************
* Adds a vector and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator+=(const Vector3_16f& rhs) {
	v = _mm_add_ps(v, rhs.v);
	return *this;
}

/**************************************************************************************************
* Subtracts a vector and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator-=(const Vector3_16f& rhs) {
	v = _mm_sub_ps(v, rhs.v);
	return *this;
}

/**************************************************************************************************
* Multiplies a vector by a scalar and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator*=(float rhs) {
	v = _mm_mul_ps(v, _mm_load1_ps(&rhs));
	return *this;
}

/**************************************************************************************************
* Multiplies a vector by a scalar and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator*=(const __m128& rhs) {
	v = _mm_mul_ps(v, rhs);
	return *this;
}

/**************************************************************************************************
* Divides a vector by a scalar and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator/=(float rhs) {
	v = _mm_div_ps(v, _mm_load1_ps(&rhs));
	return *this;
}

/**************************************************************************************************
* Divides a vector by a scalar and returns the result
**************************************************************************************************/
inline Vector3_16f& Vector3_16f::operator/=(const __m128& rhs) {
	v = _mm_div_ps(v, rhs);
	return *this;
}

/**************************************************************************************************
* Returns a positive version of the vector
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator+(void) const {
	return *this;
}

/**************************************************************************************************
* Returns a negated version of the vector
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator-(void) const {
	return Vector3_16f(_mm_mul_ps(v, _mm_set1_ps(-1.0f)));
}

/**************************************************************************************************
* Returns the sum of two vectors
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator+(const Vector3_16f& rhs) const {
	return Vector3_16f(_mm_add_ps(v, rhs.v));
}

/**************************************************************************************************
* Returns the difference between two vectors
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator-(const Vector3_16f& rhs) const {
	return Vector3_16f(_mm_sub_ps(v, rhs.v));
}

/**************************************************************************************************
* Returns a vector multiplied by a scalar
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator*(float rhs) const {
	return Vector3_16f(_mm_mul_ps(v, _mm_load1_ps(&rhs)));
}

/**************************************************************************************************
* Returns a vector multiplied by a scalar
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator*(const __m128& rhs) const {
	return Vector3_16f(_mm_mul_ps(v, rhs));
}

/**************************************************************************************************
* Returns a vector divided by a scalar
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator/(float rhs) const {
	return Vector3_16f(_mm_div_ps(v, _mm_load1_ps(&rhs)));
}

/**************************************************************************************************
* Returns a vector divided by a scalar
**************************************************************************************************/
inline Vector3_16f Vector3_16f::operator/(const __m128& rhs) const {
	return Vector3_16f(_mm_div_ps(v, rhs));
}

/**************************************************************************************************
* Returns a vector multiplied by a scalar
**************************************************************************************************/
inline Vector3_16f operator*(float lhs, const Vector3_16f& rhs) {
	return Vector3_16f(_mm_mul_ps(_mm_load1_ps(&lhs), rhs.v));
}

/**************************************************************************************************
* Returns a vector multiplied by a scalar
**************************************************************************************************/
inline Vector3_16f operator*(const __m128& lhs, const Vector3_16f& rhs) {
	return Vector3_16f(_mm_mul_ps(lhs, rhs.v));
}

/**************************************************************************************************
* Equivalence operator
**************************************************************************************************/
inline bool Vector3_16f::operator==(const Vector3_16f& rhs) const {
	return _mm_movemask_ps(_mm_cmpeq_ps(v, rhs.v)) == 0x0F;
}

/**************************************************************************************************
* Non-equivalence operator
**************************************************************************************************/
inline bool Vector3_16f::operator!=(const Vector3_16f& rhs) const {
	 return _mm_movemask_ps(_mm_cmpeq_ps(v, rhs.v)) != 0x0F;
}

/**************************************************************************************************
* Length
**************************************************************************************************/
inline float Vec3Length_16f(const Vector3_16f* pV) {
	return sqrtf(pV->x*pV->x + pV->y*pV->y + pV->z*pV->z);
}

/**************************************************************************************************
* Squared length
**************************************************************************************************/
inline float VecLengthSqr_16f(const Vector3_16f* pV) {
	return pV->x*pV->x + pV->y*pV->y + pV->z*pV->z;
}

/**************************************************************************************************
* Dot product
**************************************************************************************************/
inline float Vec3Dot_16f(const Vector3_16f* pV1, const Vector3_16f* pV2) {
	return pV1->x*pV2->x + pV1->y*pV2->y + pV1->z*pV2->z;
}

/**************************************************************************************************
* Cross product
**************************************************************************************************/
inline Vector3_16f* Vec3Cross_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2) {
	pOut->x = pV1->y*pV2->z - pV1->z*pV2->y;
	pOut->y = pV1->z*pV2->x - pV1->x*pV2->z;
	pOut->z = pV1->x*pV2->y - pV1->y*pV2->x;
	return pOut;
}

/**************************************************************************************************
* Addition of two vectors
**************************************************************************************************/
inline Vector3_16f* Vec3Add_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2) {
	pOut->v = _mm_add_ps(pV1->v, pV2->v);
	return pOut;
}

/**************************************************************************************************
* Subtraction of two vectors
**************************************************************************************************/
inline Vector3_16f* Vec3Sub_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2) {
	pOut->v = _mm_sub_ps(pV1->v, pV2->v);
	return pOut;
}

/**************************************************************************************************
* Minimize the vector
**************************************************************************************************/
inline Vector3_16f* Vec3Minimize_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2) {
	pOut->v = _mm_max_ps(pV1->v, pV2->v);
	return pOut;
}

/**************************************************************************************************
* Maximize the vector
**************************************************************************************************/
inline Vector3_16f* Vec3Maximize_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2) {
	pOut->v = _mm_min_ps(pV1->v, pV2->v);
	return pOut;
}

/**************************************************************************************************
* Scale
**************************************************************************************************/
inline Vector3_16f* Vec3Scale_16f(Vector3_16f* pOut, const Vector3_16f* pV, float s) {
	pOut->v = _mm_mul_ps(pV->v, _mm_load1_ps(&s));//_mm_set1_ps(s));
	return pOut;
}

/**************************************************************************************************
* Scale the vector
**************************************************************************************************/
inline Vector3_16f* Vec3Scale_16f(Vector3_16f* pOut, const Vector3_16f* pV, const __m128& s) {
	pOut->v = _mm_mul_ps(pV->v, s);
	return pOut;
}

/**************************************************************************************************
* Lerp V1 + s * (V2 - V1)
**************************************************************************************************/
inline Vector3_16f* Vec3Lerp_16f(Vector3_16f* pOut, const Vector3_16f* pV1, 
	const Vector3_16f* pV2, float s) {
	pOut->v = _mm_add_ps(pV1->v, _mm_mul_ps(_mm_load1_ps(&s), _mm_sub_ps(pV2->v, pV1->v)));
	return pOut;
}

/**************************************************************************************************
* Lerp V1 + s * (V2 - V1)
**************************************************************************************************/
inline Vector3_16f* Vec3Lerp_16f(Vector3_16f* pOut, const Vector3_16f* pV1, 
	const Vector3_16f* pV2, const __m128& s) {
	pOut->v = _mm_add_ps(pV1->v, _mm_mul_ps(s, _mm_sub_ps(pV2->v, pV1->v)));
	return pOut;
}

/**************************************************************************************************
* Normalize
**************************************************************************************************/
inline Vector3_16f* Vec3Normalize_16f(Vector3_16f* pOut, const Vector3_16f* pV) {
	pOut->v = _mm_div_ps(pV->v, _mm_set1_ps(Vec3Length_16f(pV)));
	return pOut;
}
